import numpy as np
import matplotlib.pyplot as plt

print("Path Show is Start")

data_dir = '../log/A1/corridor/'

# 读取相关数据
gps_path        = np.loadtxt(data_dir  + 'gt_0517_corridor_00.txt')
legodom_path    = np.loadtxt(data_dir  + 'LegOdom_align_XYZ.txt')
legkilo_path    = np.loadtxt(data_dir  + 'Leg-KILO.txt')
llio_path       = np.loadtxt(data_dir  + 'LLIO.txt')
liosam_path     = np.loadtxt(data_dir  + 'LIOSAM.txt')
pointlio_path   = np.loadtxt(data_dir  + 'Point-LIO.txt')
aloam_path      = np.loadtxt(data_dir  + 'A-LOAM.txt')
fastliosam_path = np.loadtxt(data_dir  + 'FastLioSAM.txt')




# 设置图片标题
plt.title('Path Show')
# 绘制曲线 plot(x, y, '线型', label='标签')
plt.plot(gps_path[:,1],         gps_path[:,2],          '-',  label='true')
plt.plot(legodom_path[:,1],     legodom_path[:,2],      '-',  label='LegOdom')
plt.plot(legkilo_path[:,1],     legkilo_path[:,2],      '-',  label='Leg-KILO')
plt.plot(llio_path[:,1],        llio_path[:,2],         '-',  label='LLIO')
# plt.plot(liosam_path[:,1],      liosam_path[:,2],       '-',  label='LIO-SAM')
plt.plot(pointlio_path[:,1],    pointlio_path[:,2],     '-',  label='Point-LIO')
plt.plot(aloam_path[:,1],       aloam_path[:,2],        '-',  label='A-LOAM')
plt.plot(fastliosam_path[:,1],  fastliosam_path[:,2],   '-',  label='FastLioSAM')




plt.gca().set_aspect(1)         # 设置横轴纵轴的尺度统一
plt.legend(loc='upper left')   # 设置图例（右上角）
plt.grid()



print("Read Data is Success")
plt.show()